/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMMTools
* @author     Nikolaus Vahrenkamp
* @copyright  2013 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#ifndef __MMM_ConverterVicon2MMM_H_
#define __MMM_ConverterVicon2MMM_H_

#include <Eigen/Core>
#include <string> 
#include <vector>
#include <map>

#include <MMM/MMMCore.h>
#include <MMM/MMMImportExport.h>
#include <MMM/Motion/Legacy/AbstractMotion.h>
#include <MMM/Motion/Legacy/MarkerMotion.h>
#include <MMM/Model/Model.h>
#include <MMM/Motion/Legacy/Converter/MarkerBasedConverter.h>
#include <MMM/Motion/Legacy/Converter/ConverterFactory.h>
#include <MMMSimoxTools/RobotPoseDifferentialIK.h>
#include <VirtualRobot/VirtualRobotCommon.h>
#include <VirtualRobot/Nodes/Sensor.h>

#include "ConverterVicon2MMMImportExport.h"

#include <boost/extension/extension.hpp>
#include <boost/extension/factory.hpp>
#include <boost/extension/type_map.hpp>

namespace MMM
{

/*!
	\brief A standard converter for converting Vicon marker motions to the MMM format.
*/
class ConverterVicon2MMM_IMPORT_EXPORT ConverterVicon2MMM : public MarkerBasedConverter
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	ConverterVicon2MMM(const std::string &name = "ConverterVicon2MMM");

    //! Returns true when setup was already performed.
    virtual bool isInitialized();

	/*!
	Basic setup of a converter
	\param inputModel Specifies to which model the inputMotion is linked. In case MarkerMotions are used as input (e.g. Vicon->MMM) the input model can be empty.
	\param inputMotion The motion to convert.
	\param outputModel Setup the target model.
	\return true on success.
	*/
	virtual bool setup(ModelPtr inputModel, AbstractMotionPtr inputMotion, ModelPtr outputModel);


	/*!
		Convert complete motion
	*/
	virtual AbstractMotionPtr convertMotion();

	/*!
		Some converters may offer a stepwise conversion. 
		This method initializes the output for stepwise conversion
	*/
	virtual AbstractMotionPtr initializeStepwiseConversion();

	/*!
		Pass a partially converted motion in order to compute the next frame.
        The value of currentOutput->getNumFrames() is checked in order to determine the current position of the outputMotion.
        Must be smaller than inputMotion->getNumFrames().
	*/
    virtual bool convertMotionStep(AbstractMotionPtr currentOutput);

    virtual bool convertMotionStep(AbstractMotionPtr currentOutput, bool increment);

protected:
    //! first tag in xml configuration
    virtual bool _setup(rapidxml::xml_node<char>* rootTag);

    virtual bool buildModel(MMM::ModelPtr model);

	//! Computes the center of all markers and moves model to this position
    bool moveModelToCenter(MMM::LegacyMarkerDataPtr frame);

	//! Computes the average and maximum distance of the markers of the current model position to the given frame f.
    void getDistance(MMM::LegacyMarkerDataPtr f, float &maxD, float &avgD);

	//! Fits model to frame f while considering the configurated marker mappings
    bool fitModel(MMM::LegacyMarkerDataPtr f, bool quickImprovementCheck = true, float ikStepSize = 0.2f, float ikMinChange = 0.1f, int ikSteps = 10, bool performMinOneStep = true, bool boxConstraints = true);
	
	//! Searches best intial rotation of model at curent position (assuming z == upright). Considers markermapping for distance computation.
    bool findBestModelRotation(MMM::LegacyMarkerDataPtr f, int nrRotationsToCheck = 10);

	VirtualRobot::RobotPtr mmmModel; // the simox model 
	VirtualRobot::RobotNodeSetPtr rns; // the joints to manipulate
    std::map<std::string, VirtualRobot::SensorPtr> modelMarkers; // the markers on the mmm model

    RobotPoseDifferentialIKPtr ik;
public:
	float paramIKStepSize;
	float paramIKMinChange;
	int paramIKSteps;
    bool paramCheckImprovement;
    bool paramPerfomMinOneStep;
    bool paramJointLimitsBoxConstraints;
	float paramInitialIKStepSize;
    float paramInitialIKMinChange;
    int paramInitialIKSteps;
    bool paramInitialCheckImprovement;
    bool paramInitialPerfomMinOneStep;
    bool paramInitialJointLimitsBoxConstraints;

	std::string modelName;
    Eigen::MatrixXf _jacobian;

};

typedef boost::shared_ptr<ConverterVicon2MMM> ConverterVicon2MMMPtr;

}

#endif 
